
#include <stdlib.h>
#include <math.h>

#include <util/common_macros.h>
#include <util/msg.h>
#include <util/math_macros.h>
#include <util/mem.h>

#include "../image.h"
#include "../../matrix/matrix.h"
#include "../../dsp/dsp.h"

static real * _cosr = NULL;
static real * _sinr = NULL;
static real * _hfrac = NULL;
static real * _vfrac = NULL;
static int * __create_int_sequence(int length, int start, int increment) {
	int i, inc;
	int * shift;

	//shift = calloc(length, sizeof (int));
	shift = mem_allocn(length, int);
	for (i = 0, inc = start; i < length; i++, inc += increment) shift[i] = inc;

	return shift;
}
static void __compute_xy_components(real ray) {

	int i;
	int max_ang;
	real rad;

	if (_cosr != NULL) {
		/* Just computed! */
		return;
	}

	max_ang = 180;

	_cosr = (real *) calloc(max_ang + 1, sizeof (real));
	_sinr = (real *) calloc(max_ang + 1, sizeof (real));
	_hfrac = (real *) calloc(max_ang + 1, sizeof (real));
	_vfrac = (real *) calloc(max_ang + 1, sizeof (real));

	for (i = 0; i < max_ang + 1; i++) {

		rad = deg2rad(i);

		_cosr[i] = ray * cos(rad); // x component
		_sinr[i] = ray * sin(rad); // y component

		_hfrac[i] = _cosr[i] - floor(_cosr[i]); // (x - x1) x difference to linear interpolation
		_vfrac[i] = _sinr[i] - floor(_sinr[i]); // (y - y1) y difference to linear interpolation
	}
}
/**
 * \brief This function realize the nom-maxima suppression to edge detection.
 * 
 * This function uses bilinear interpolation.
 *
 * @param Mag
 * @param Or
 * @param ray
 * @param NMS
 */
void imp_image_nonmaxsup_edge(ImpMatrix * Mag, ImpMatrix * Or, real ray, ImpMatrix ** NMS) {

	/*  +===========================================+
	 *  |                                           |
	 *  |          Bilinear Interpolation           |
	 *  |                                           |
	 *  |                                           |
	 *  |      (0,0)   fx            cx    x        |
	 *  |       -|-----|-------------|------>       |
	 *  |        |                                  |
	 *  |        |                                  |
	 *  |        |            x                     |
	 *  |        |            |                     |
	 *  |        |     q11    bi     q21            |
	 *  |     fy -     o------o------o              |
	 *  |        |     |      |      |              |
	 *  |        |     |      |      |              |
	 *  |        |     |      |      |              |
	 *  |        |  y --------o      |              |
	 *  |        |     |      |      |              |
	 *  |        |     |      |      |              |
	 *  |        |     |      |      |              |
	 *  |     cy -     o------o------o              |
	 *  |        |     q12    ti     q22            |
	 *  |        |                                  |
	 *  |        |                                  |
	 *  |     y  |                                  |
	 *  |        v                                  |
	 *  |                                           |
	 *  |     |_________________________________|   |
	 *  |                    ImpImage               |
	 *  |                                           |
	 *  +-------------------------------------------+
	 */

	real * _mag;
	real * _or;
	real * _nms;

	int rows;
	int columns;

	int r, c, _r, i;

	real x, y;

	int fx, cx;
	int fy, cy;

	real q11, q21, q12, q22;

	real ti, bi, vi;

	real angle;
	int deg;

	_verify(NMS);
	if (*NMS == NULL) (*NMS) = imp_matrix_new(Mag->rows, Mag->columns);

	_mag = Mag->data;
	_or = Or->data;
	_nms = (*NMS)->data;

	rows = Mag->rows;
	columns = Mag->columns;

	// compute x and y components from ray for each angle [0 - 180]
	__compute_xy_components(ray);

	for (r = 0, _r = r * columns; r < rows - 1; r++, _r += columns) {
		for (c = 0; c < columns - 1; c++) {
			i = _r + c;

			/*= Verify the first pixel in gradient direction ================ */
			angle = _or[i];

			/* Convert all angles greater than 180 to the range [0 - 180] to 
			 * avoid invalid index */
			if (angle > M_PI) angle = angle - M_PI;

			angle = rad2deg(angle);

			/* Adjust the right index */
			deg = FIX(angle);

			x = c + _cosr[ deg ];
			y = r + _sinr[ deg ];

			fx = FLOOR(x);
			cx = CEIL(x);
			fy = FLOOR(y);
			cy = CEIL(y);

			/* Avoid invalid indexes */
			if (fx < 0 || fx >= columns) continue;
			if (cx < 0 || cx >= columns) continue;
			if (fy < 0 || fy >= rows) continue;
			if (cy < 0 || cy >= rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			bi = q11 + _hfrac[ deg ] * (q21 - q11);
			ti = q12 + _hfrac[ deg ] * (q22 - q12);
			vi = bi + _vfrac[ deg ] * (ti - bi); // Bilinear interpolation

			if (_mag[i] <= vi) continue; // _mag[i] is non-maxima

			/*= Verify the second pixel in opposite gradient direction ====== */
			x = c - _cosr[ deg ];
			y = r - _sinr[ deg ];

			fx = FLOOR(x);
			cx = CEIL(x);
			fy = FLOOR(y);
			cy = CEIL(y);

			/* Avoid invalid indexes */
			if (fx < 0 || fx >= columns) continue;
			if (cx < 0 || cx >= columns) continue;
			if (fy < 0 || fy >= rows) continue;
			if (cy < 0 || cy >= rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			bi = q11 + _hfrac[ deg ] * (q21 - q11);
			ti = q12 + _hfrac[ deg ] * (q22 - q12);
			vi = bi + _vfrac[ deg ] * (ti - bi); // Bilinear interpolation

			if (_mag[i] > vi) {
				_nms[i] = _mag[i]; /* _mag[i] is a maxima */
			}

		}
	}

}
static int __image_nonmaxsup_feature_without_mask(ImpMatrix * Mat, ImpMatrix * Domine, ImpMatrix * NMS) {

	// =========================================================================
	real * mat = NULL;
	real * nms = NULL;
	int rows, columns;
	int r, c, _r, i;

	mat = Mat->data;
	nms = NMS->data;
	rows = Mat->rows;
	columns = Mat->columns;

	// =========================================================================
	real * domine = NULL;
	int mrows, mcolumns;
	int mr, mc, _mr, mi;

	domine = Domine->data;
	mrows = Domine->rows;
	mcolumns = Domine->columns;

	// =========================================================================
	int rf, cf;
	int mrc = (mrows - 1) >> 1;
	int mcc = (mcolumns - 1) >> 1;
	int _2columns_minus_2 = (columns << 1) - 2;
	int _2row_minus_2 = (rows << 1) - 2;
	int * r_shift = __create_int_sequence(rows, 0, columns);
	int d;

	// =========================================================================
	real MAX;
	int qtd = 0;

	// =========================================================================
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			// = For each pixel of Mat do: =====================================

			MAX = mat[i];

			for (mr = 0, _mr = mr * mcolumns; mr < mrows; mr++, _mr += mcolumns) {
				// = Reflect y boundary ========================================
				rf = r + (mr - mrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2row_minus_2 - rf;

				for (mc = 0; mc < mcolumns; mc++) {
					mi = _mr + mc;
					// = For each element in the mask do: ======================

					if (!domine[mi]) continue;

					// = Reflect x boundary ====================================
					cf = c + (mc - mcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					d = r_shift[rf] + cf;

					// = Operations ============================================
					if (mat[d] > MAX) MAX = mat[d];
				}
			}

			// = If MAX is the current pixel, then it is a local maximum =======
			if (mat[i] == MAX) {
				nms[i] = MAX;
				qtd++;
			}
		}
	}
	return qtd;
}
static int __image_nonmaxsup_feature_with_mask(ImpMatrix * Mat, ImpMatrix * Domine, ImpMatrix * Mask, ImpMatrix * NMS) {

	// =========================================================================
	real * mat = NULL;
	real * nms = NULL;
	real * mask = NULL;
	int rows, columns;
	int r, c, _r, i;

	mat = Mat->data;
	nms = NMS->data;
	mask = Mask->data;
	rows = Mat->rows;
	columns = Mat->columns;

	// =========================================================================
	real * domine = NULL;
	int mrows, mcolumns;
	int mr, mc, _mr, mi;

	domine = Domine->data;
	mrows = Domine->rows;
	mcolumns = Domine->columns;

	// =========================================================================
	int rf, cf;
	int mrc = (mrows - 1) >> 1;
	int mcc = (mcolumns - 1) >> 1;
	int _2columns_minus_2 = (columns << 1) - 2;
	int _2row_minus_2 = (rows << 1) - 2;
	int * r_shift = __create_int_sequence(rows, 0, columns);
	int d;

	// =========================================================================
	real MAX;
	int qtd = 0;

	// =========================================================================
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			// = For each pixel of Mat do: =====================================

			if (mask[i] == 0) continue;

			MAX = mat[i];

			for (mr = 0, _mr = mr * mcolumns; mr < mrows; mr++, _mr += mcolumns) {
				// = Reflect y boundary ========================================
				rf = r + (mr - mrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2row_minus_2 - rf;

				for (mc = 0; mc < mcolumns; mc++) {
					mi = _mr + mc;
					// = For each element in the mask do: ======================

					if (!domine[mi]) continue;

					// = Reflect x boundary ====================================
					cf = c + (mc - mcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					d = r_shift[rf] + cf;

					// = Operations ============================================
					if (mat[d] > MAX) MAX = mat[d];
				}
			}

			// = If MAX is the current pixel, then it is a local maximum =======
			if (mat[i] == MAX) {
				nms[i] = MAX;
				qtd++;
			}
		}
	}
	return qtd;
}
int imp_image_nonmaxsup_feature(ImpMatrix * Mat, int ray, ImpMatrix * Mask, ImpMatrix ** NMS) {
	_exit_if_fail(Mat);
	_exit_if_fail(ray > 0);
	_exit_if_fail(NMS);

	int qtd = 0;
	ImpMatrix * Domine;
	Domine = imp_matrix_new_ones(1 + (ray << 1), 1 + (ray << 1));
	if (*NMS == NULL) (*NMS) = imp_matrix_new(Mat->rows, Mat->columns);

	if (Mask) qtd = __image_nonmaxsup_feature_with_mask(Mat, Domine, Mask, *NMS);
	else qtd = __image_nonmaxsup_feature_without_mask(Mat, Domine, *NMS);

	imp_object_delete(Domine);

	return qtd;
}

/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
int old_imp_image_nonmaxsup_feature(ImpMatrix * Mat, int ray, ImpMatrix ** NMS) {

	real * mat = NULL;
	real * nms = NULL;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;
	long int qtd;

	int d;

	real MAX;

	{ /* Parameters verification ============================================ */
		_verify(Mat);
		_verify(NMS);
		_verify(ray > 0);
	}

	{ /* Initialization ===================================================== */
		mat = Mat->data;
		rows = Mat->rows;
		columns = Mat->columns;
	}

	{ /* Memory Allocation ================================================== */
		if (*NMS == NULL) (*NMS) = imp_matrix_new(rows, columns);
		nms = (*NMS)->data;
	}


	{ /* Verify pixels in SE ================================================ */

		ImpMatrix * SE = imp_matrix_new_ones(1 + ray * 2, 1 + ray * 2);
		real * se = SE->data;
		int mrows = SE->rows;
		int mcolumns = SE->columns;

		int rf, cf;

		int mrc = (mrows - 1) / 2;
		int mcc = (mcolumns - 1) / 2;

		int _2columns_minus_2 = (columns << 1) - 2;
		int _2row_minus_2 = (rows << 1) - 2;

		int * r_shift = __create_int_sequence(rows, 0, columns);

		qtd = 0;

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;

				MAX = mat[i];

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					for (mc = 0; mc < mcolumns; mc++) {

						mi = _mr + mc;

						if (!se[mi]) continue;

						// reflect x boundaries
						cf = c + (mc - mcc);
						if (cf < 0) cf = -cf;
						else if (cf >= columns) cf = _2columns_minus_2 - cf;

						d = r_shift[rf] + cf;

						if (mat[d] > MAX) MAX = mat[d];
					}
				}
				if (mat[i] == MAX) {
					nms[i] = MAX;
					qtd++;
				}
			}
		}// </editor-fold>
		free(r_shift);
	}
	return qtd;
}
int imp_image_nonmaxsup_feature_nonzero(ImpMatrix * Mat, int ray, ImpMatrix ** NMS) {

	real * mat = NULL;
	real * nms = NULL;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;
	long int qtd;

	int d;

	real MAX;

	{ /* Parameters verification ============================================ */
		_verify(Mat);
		_verify(NMS);
		_verify(ray > 0);
	}

	{ /* Initialization ===================================================== */
		mat = Mat->data;
		rows = Mat->rows;
		columns = Mat->columns;
	}

	{ /* Memory Allocation ================================================== */
		if (*NMS == NULL) (*NMS) = imp_matrix_new(rows, columns);
		nms = (*NMS)->data;
	}


	{ /* Verify pixels in SE ================================================ */

		ImpMatrix * SE = imp_matrix_new_ones(1 + ray * 2, 1 + ray * 2);
		real * se = SE->data;
		int mrows = SE->rows;
		int mcolumns = SE->columns;

		int rf, cf;

		int mrc = (mrows - 1) / 2;
		int mcc = (mcolumns - 1) / 2;

		int _2columns_minus_2 = (columns << 1) - 2;
		int _2row_minus_2 = (rows << 1) - 2;

		int * r_shift = __create_int_sequence(rows, 0, columns);

		qtd = 0;

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {


				if (r == 186 && c == 390) {
					r = 186;
				}

				i = _r + c;

				MAX = mat[i];

				if (mat[i] == 0) continue;

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					for (mc = 0; mc < mcolumns; mc++) {

						mi = _mr + mc;

						if (!se[mi]) continue;

						// reflect x boundaries
						cf = c + (mc - mcc);
						if (cf < 0) cf = -cf;
						else if (cf >= columns) cf = _2columns_minus_2 - cf;

						d = r_shift[rf] + cf;

						if (mat[d] > MAX) MAX = mat[d];
						//else if( mat[d] == MAX ) 
					}
				}
				if (mat[i] == MAX) {
					nms[i] = MAX;
					qtd++;
				}
			}
		}// </editor-fold>
		free(r_shift);
	}
	return qtd;
}
void imp_image_extract_coordinates(ImpMatrix * Mat, real Th, ImpMatrix ** Coord) {

	real * mat;
	real * coord;

	int rows;
	int columns;

	int r, c, _r;
	long int i;
	long int qtd;

	_verify(Mat);
	_verify(Coord);

	mat = Mat->data;
	rows = Mat->rows;
	columns = Mat->columns;

	qtd = 0;

	for (i = 0; i < Mat->size; i++) {
		if (mat[i] > Th) qtd++;
	}

	(*Coord) = imp_matrix_new(qtd, 2);
	coord = (*Coord)->data;

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {

			i = _r + c;

			if (mat[i] > Th) {
				*coord = c;
				coord++;
				*coord = r;
				coord++;
			}
		}
	}
}
void OK_imp_nonmaxsup(ImpMatrix * Mag, ImpMatrix * Or, real ray, ImpMatrix ** NMS) {

	real * _mag;
	real * _or;
	real * _nms;

	int rows;
	int columns;

	int r, c, _r, i;

	real x, y;

	int fx, cx;
	int fy, cy;

	real q11, q21, q12, q22;

	real ti, bi, vi;

	real _cosr[180 + 1];
	real _sinr[180 + 1];

	real _hfrac[180 + 1];
	real _vfrac[180 + 1];

	real rad;
	real angle;

	_verify(NMS);
	if (*NMS == NULL) (*NMS) = imp_matrix_new(Mag->rows, Mag->columns);

	_mag = Mag->data;
	_or = Or->data;
	_nms = (*NMS)->data;

	rows = Mag->rows;
	columns = Mag->columns;

	// compute x and y components from ray for each angle [0 - 180]
	for (i = 0; i < 180 + 1; i++) {
		rad = i * M_PI / 180.0;
		_cosr[i] = ray * cos(rad); // x component
		_sinr[i] = ray * sin(rad); // y component

		_hfrac[i] = _cosr[i] - floor(_cosr[i]);
		_vfrac[i] = _sinr[i] - floor(_sinr[i]);
	}

	for (r = 1, _r = columns; r < rows - 1; r++, _r += columns) {
		for (c = 1; c < columns - 1; c++) {
			i = _r + c;

			if (r == 62 && c == 86) {
				//matrix_print3x3(Or, c, r);
			}

			//= Verify the first pixel in gradient direction ================ 
			angle = _or[i] * 180 / M_PI;
			if (angle < 0) angle = angle + 180;
			angle = FIX(angle);

			x = c + _cosr[ (int) angle ];
			y = r - _sinr[ (int) angle ];

			//
			fx = FLOOR(x);
			cx = CEIL(x);
			fy = FLOOR(y);
			cy = CEIL(y);

			fx = floor(x);
			cx = ceil(x);
			fy = floor(y);
			cy = ceil(y);


			// Avoid invalid indexes 
			if (fx < 0 || fx > columns) continue;
			if (cx < 0 || cx > columns) continue;
			if (fy < 0 || fy > rows) continue;
			if (cy < 0 || cy > rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			//
			bi = q11 + (x - fx) * (q21 - q11);
			ti = q12 + (x - fx) * (q22 - q12);
			vi = bi + (1 - (y - fy)) * (ti - bi); // Bilinear interpolation


			bi = q11 + _hfrac[(int) angle] * (q21 - q11);
			ti = q12 + _hfrac[(int) angle] * (q22 - q12);
			vi = bi + _vfrac[(int) angle] * (ti - bi); // Bilinear interpolation

			if (_mag[i] <= vi) continue; // _mag[i] is non-maxima

			//= Verify the second pixel in opposite gradient direction ====== 
			x = c - _cosr[ (int) angle ];
			y = r + _sinr[ (int) angle ];

			fx = floor(x);
			cx = ceil(x);
			fy = floor(y);
			cy = ceil(y);

			// Avoid invalid indexes 
			if (fx < 0 || fx > columns) continue;
			if (cx < 0 || cx > columns) continue;
			if (fy < 0 || fy > rows) continue;
			if (cy < 0 || cy > rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			//
			bi = q11 + (1 - (x - fx)) * (q21 - q11);
			ti = q12 + (1 - (x - fx)) * (q22 - q12);
			vi = bi + (y - fy) * (ti - bi); // Bilinear interpolation

			bi = q11 + _hfrac[(int) angle] * (q21 - q11);
			ti = q12 + _hfrac[(int) angle] * (q22 - q12);
			vi = bi + _vfrac[(int) angle] * (ti - bi); // Bilinear interpolation

			if (_mag[i] > vi) {
				_nms[i] = _mag[i]; // _mag[i] is a maxima 
			}

		}
	}

}
void image_nonmaxsup_feature_zb(ImpMatrix * Mat, int ray, ImpMatrix ** NMS) {

	ImpMatrix * SE = NULL;

	ImpMatrix * Shift = NULL;
	real * shift;

	real * mat;
	real * nms;
	real * se;

	int rows;
	int columns;
	long int imsize;

	int mrows;
	int mcolumns;

	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 
	int d;

	real MAX;

	_verify(NMS);
	if (NMS) if (*NMS == NULL) (*NMS) = imp_matrix_new(Mat->rows, Mat->columns);

	SE = imp_matrix_new_ones(1 + ray * 2, 1 + ray * 2);

	mat = Mat->data;
	nms = (*NMS)->data;
	se = SE->data;

	rows = Mat->rows;
	columns = Mat->columns;
	imsize = Mat->size;

	mrows = SE->rows;
	mcolumns = SE->columns;

	//Shift = matrix_create_conv_index_shift(SE->rows, SE->columns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;

			MAX = mat[i];

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;

					if (!se[mi]) continue;

					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					if (mat[d] > MAX) MAX = mat[d];

				}
			}
			if (mat[i] == MAX) nms[i] = MAX;
			//nms[i] = MAX;

			//if (mat[i] == MAX && mat[i] > 600) nms[i] = 255;
			//else nms[i] = 0;

		}
	}
	imp_object_delete(Shift);
}
